package CamposDePotencial;

import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.IOException;
import java.io.InputStream;
import java.util.Properties;

import javax.vecmath.Point3d;

import estrategia.ParametrosPotenciales;
import estrategia.Predictor;
import estrategia.Utiles;
import estrategia.Vector;

public class CampoTangencial extends CampoPotencial{

	private double radioCampo;
	private double minAlcance;
	private double Krepulsion;
	private double tamanioRobot;
	
	private Objetivo objetivo;
	
	public CampoTangencial(int idObjeto, Objetivo objetivo, Utiles u, Predictor predictor) {
		// TODO Auto-generated constructor stub
		super(idObjeto, u, predictor);
		this.objetivo = objetivo;
	}
	
	
	public void  cargarConstantes() {
	
		radioCampo = ParametrosPotenciales.radioCampoTangencial;
		minAlcance = ParametrosPotenciales.minAlcanceTangencial;
		Krepulsion = ParametrosPotenciales.KCampoTangencial;
		tamanioRobot = ParametrosPotenciales.tamanioRobot;
		
	}
	
	
	public double getMagnitudFuerza(Point3d mipos, Point3d posObj) {
		double magnitud = 0;
		double distancia = utiles.distanciaEuclidea(mipos, posObj);
		distancia = distancia - tamanioRobot;
		
		if (distancia <= radioCampo) {
			// *(1/(Math.pow(distancia, 2)))
			magnitud = Krepulsion*((1/distancia) - (1/radioCampo));
		}
		
		return magnitud;
	}
	
	
	public double obtenerNuevosEjes() {
		double angulo0 = 0;
		
		double thetaObjetivo = 0;
		try {
			
			Point3d posObjetivo = objetivo.getPosicionObjetivo();
			Point3d posObjeto = predictor.predecirPosicion(idObjeto);
			thetaObjetivo = utiles.getAnguloTheta(posObjeto, posObjetivo);
			System.out.println("thetaObjetivo " + thetaObjetivo + "idO = " + idObjeto + "idG" + objetivo.getIdObjetivo());
		} catch (IOException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		
		//angulo0 = -Math.signum(thetaObjetivo)*Math.PI + thetaObjetivo;
		if (thetaObjetivo < -Math.PI/2 && thetaObjetivo >= -Math.PI)
			// es 180 menos lo que se paso de 90
			angulo0 = Math.PI + (thetaObjetivo + Math.PI/2);
		else
			angulo0 = thetaObjetivo - Math.PI/2;
		
		return angulo0;
	}
	
	public double calcularThetaNuevosEjes(double thetaXY) {
		double thetaNuevo = 0;
		// obtengo el 0 en los nuevos ejes dados pro el objetivo
		double angulo0 = obtenerNuevosEjes();
		System.out.println("angulo 0 " + angulo0 + "en objeto " + idObjeto);
		
		thetaNuevo = thetaXY - angulo0;
		
		if (Math.signum(thetaXY) != Math.signum(angulo0)) {
			
			if (thetaXY >= 0 && (thetaXY - angulo0 > Math.PI)) {
				thetaNuevo = -Math.PI + (thetaXY - angulo0 - Math.PI);
			}
			
			if (angulo0 >= 0 && (angulo0 - thetaXY > Math.PI)) {
				thetaNuevo = Math.PI - (angulo0 - thetaXY - Math.PI);
			} 
		}
		
		return thetaNuevo;
	}
	
	public double calcularDireccion(Point3d mipos) throws IOException {
		double direccion = 0;
		
		double thetaXYOrigenObstaculo = utiles.getAnguloTheta(predictor.predecirPosicion(idObjeto), mipos);
		System.out.println("theta xy = " + thetaXYOrigenObstaculo);
		double thetaXYNuevosOrigenObstaculo = calcularThetaNuevosEjes(thetaXYOrigenObstaculo);
		System.out.println("theta xy nuevo = " + thetaXYNuevosOrigenObstaculo);	
		double thetaXYNuevosOrigenRobot = 0;
			
		if (thetaXYNuevosOrigenObstaculo >= 0) {
			thetaXYNuevosOrigenRobot = -Math.PI + thetaXYNuevosOrigenObstaculo;
		} else {
			thetaXYNuevosOrigenRobot = Math.PI + thetaXYNuevosOrigenObstaculo;
		}
		
		// No use el signum por el caso del cero
		if (thetaXYNuevosOrigenRobot >= 0) {
			if (thetaXYNuevosOrigenRobot <= Math.PI/2)
				direccion = thetaXYNuevosOrigenRobot + Math.PI/2;
			else
				direccion = thetaXYNuevosOrigenRobot - Math.PI/2;
		} else {
			if (thetaXYNuevosOrigenRobot >= -Math.PI/2)
				direccion = thetaXYNuevosOrigenRobot + Math.PI/2;
			else {
				if (thetaXYNuevosOrigenRobot - Math.PI/2 < -Math.PI)
					direccion = Math.PI + (thetaXYNuevosOrigenRobot - Math.PI/2 + Math.PI);
				else 
					direccion = (thetaXYNuevosOrigenRobot - Math.PI/2);
			}
			
		}
		
		
		return direccion;
	}
	
	
	public Vector obtenerFuerza(Point3d mipos) throws IOException{
		cargarConstantes();
		
		double direccion = calcularDireccion(mipos);
		
		double magnitud = getMagnitudFuerza(mipos, predictor.predecirPosicion(idObjeto));
		
		System.out.println("REPULSOR tangencial " + idObjeto + ": direccion = " + direccion + " magnitud = " + magnitud);
		 
		Vector resultante = new Vector(magnitud, direccion);  
		
		resultante.setMagnitud(Math.abs(magnitud));
		
		return resultante;
	};
}

